/*
 * HalcyonV1X3.cpp
 *
 * Created: 28.04.2012 10:11:25
 *  Author: Maxim Karban <maxim.karban@gmail.com>
 */ 

#include "halcyon_v1.h"

#include <stdint.h>
#include <stdbool.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
#include <util/delay.h>

#include <stdlib.h>
#include <string.h>

#include "i2c_master.h"

#if defined(BARO)
  #include "bosch_pressure.h"
#endif //BARO

#if defined(ACCEL)
  #include "mma7455.h"
#endif //ACCEL

#if defined(GPS)
  #include "gps.h"
#endif //GPS

#include "uart.h"
#include "frsky.h"
#include "sys_events.h"
#include "halcyon_init.h"

#include "filter.h"

#include "debug.h"

int16_t accel_x = 0;
int16_t accel_y = 0;
int16_t accel_z = 0;

int16_t H_bp = 0;
uint16_t H_ap = 0;



int32_t Hfiltered = 0;
int32_t Hfinal = 0;

int main( void )
{
  initHW();

  sei();

  while(1){
//all printFrSky.. takes max 0.7ms
#if defined(BARO)
    if(hLockData & PRESSURE_DATA_READY){//1.2ms here
      //debug_pin_on;
      Hfinal = BSCalcAltitude();
      Hfiltered = altitudeFilter(Hfinal);
      hLockData &= ~PRESSURE_DATA_READY;
      hLockData |= ALTITUDE_DATA_READY;
      //debug_pin_off;
    }
#endif //BARO

#if defined(ACCEL)
    if(hLockData & ACCEL_DATA_READY){//0.16ms here
      //debug_pin_on;
      MMADecode(&accel_x, &accel_y, &accel_z);
      MMAFilter(&accel_x, &accel_y, &accel_z);
      hLockData &= ~ACCEL_DATA_READY;
      hLockData |= ACCEL_FILTER_READY;
      //debug_pin_off;
    }
#endif //ACCEL

    if(hLockTime & MARK_200MS){
      //frSky frame 1
      
#if defined(ACCEL)
      if(hLockData & ACCEL_FILTER_READY){
        //debug_pin_on;
        printFrSkyValueInt(accel_x, FRSKY_ACCEL_X);
        printFrSkyValueInt(accel_y, FRSKY_ACCEL_Y);
        printFrSkyValueInt(accel_z, FRSKY_ACCEL_Z);
        hLockData &= ~ACCEL_FILTER_READY;
        hLockData |= ANY_DATA_PRINTED;
        //debug_pin_off;
      }
#endif //ACCEL

#if defined(BARO)
      if(hLockData & ALTITUDE_DATA_READY){
        //debug_pin_on;
        H_bp = Hfiltered/100;
        H_ap = abs(Hfiltered)-abs((int32_t)(Hfiltered/100))*100;
        printFrSkyValueInt(H_bp, FRSKY_BARO_ALTITUDE_BP);
        printFrSkyValueUInt(H_ap, FRSKY_BARO_ALTITUDE_AP);
        hLockData &= ~ALTITUDE_DATA_READY;
        hLockData |= ANY_DATA_PRINTED;
        //debug_pin_off;
      }
#endif //BARO

#if defined(BARO) | defined(ACCEL)
      if(hLockData & ANY_DATA_PRINTED){
        //debug_pin_on;
        printFrSkyEOF();
        //debug_pin_off;
        hLockData &= ~ANY_DATA_PRINTED;
      }      
#endif //BARO | ACCEL

      hLockTime &= ~MARK_200MS;
    }
    
    
    if(hLockTime & MARK_1S){
#if defined(GPS)
      //debug_pin_on;

      while(hLockData & GPS_DATA_LOCKED);
      memcpy(&gpsData.gpsNavDataCache, &gpsData.gpsNavData, sizeof(gpsNavData_t));

      //frSky frame 2
      //Course
      printFrSkyValueInt(gpsData.gpsNavDataCache.course_bp, FRSKY_GPS_COURSE_BP);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.course_ap, FRSKY_GPS_COURSE_AP);
      //Latitude
      printFrSkyValueInt(gpsData.gpsNavDataCache.lat_bp, FRSKY_GPS_LAT_BP);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.lat_ap, FRSKY_GPS_LAT_AP);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.lat_ns, FRSKY_GPS_LAT_NS);
      //Longitude
      printFrSkyValueInt(gpsData.gpsNavDataCache.lon_bp, FRSKY_GPS_LONG_BP);
      printFrSkyValueInt(gpsData.gpsNavDataCache.lon_ap, FRSKY_GPS_LONG_AP);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.lon_ew, FRSKY_GPS_LONG_EW);
      //Speed
      printFrSkyValueInt(gpsData.gpsNavDataCache.speed_bp, FRSKY_GPS_SPEED_BP);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.speed_ap, FRSKY_GPS_SPEED_AP);
      //Altitude GPS
      printFrSkyValueInt(gpsData.gpsNavDataCache.alt_bp, FRSKY_GPS_ALTITUDE_BP);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.alt_ap, FRSKY_GPS_ALTITUDE_AP);
        
      printFrSkyEOF();
          
      //frSky frame 3
      // must be sent each 5s but for now each 1s
      //Date
      printFrSkyValueUInt(gpsData.gpsNavDataCache.date_day_month, FRSKY_GPS_DATE_MONTH);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.date_year, FRSKY_GPS_DATE_YEAR);
      //Time
      printFrSkyValueUInt(gpsData.gpsNavDataCache.time_hour_min, FRSKY_GPS_HOUR_MIN);
      printFrSkyValueUInt(gpsData.gpsNavDataCache.time_sec, FRSKY_GPS_SEC);

      printFrSkyEOF();
        
      //debug_pin_off;
#endif //GPS          
      hLockTime &= ~MARK_1S;
    }

  }  

}
